#include "APP.h"


extern OpenMV_typedef openmv;
extern PID_InitDefStruct p_st_ang, p_st_in;
extern Timer_typeDef timer;
extern TARGET_typeDef target;
//extern ENCODER_typeDef encoder1, encoder2;

//extern int stepper_left_circle, stepper_right_circle;
extern float yaw_target;

extern PID_Mode current_pid_mode;
extern Timer_typeDef timer;


void APP_Init(void)
{
//	OLED_Init();
//	MPU6050_initialize();
//	DMP_Init();
//	ENCODER_Init();
//	MOTOR_Init();
//	// STEPPER_Init();
//	// STEER_Init();
//	// APP_GPIO_Init();	// 解决烧录问题的
//	Timer_Start();
//	OLED_ShowNum(1,12,1,1);
}


void APP_stateJudge(uint32_t count, stateJudge_mode mode, int pulse)
{
	switch (mode)
	{
		case TIMEOUT:
			openmv.cross_flag = 0;
			timer.ms_count = 0;
			while(timer.ms_count  <= count )
					delay_ms(20);
//			OLED_ShowNum(3,1,timer.count,4);
	 		timer.ms_count = 0;
	 		openmv.cross_flag = 0;
			openmv.cross_num++;
				break;

		case CROSS:		
			openmv.cross_flag = 0;
			timer.ms_count = 0;
			while(openmv.cross_flag == 0)
					delay_ms(20);
	 		timer.ms_count = 0;
	 		openmv.cross_flag = 0;
			openmv.cross_num++;
				break;

		case TIMEOUT_CROSS:		
			openmv.cross_flag = 0;
			timer.ms_count = 0;
			while(timer.ms_count  <= count &&
	 	   		  openmv.cross_flag == 0)
					delay_ms(20);
	 		timer.ms_count = 0;
	 		openmv.cross_flag = 0;
			openmv.cross_num++;
				break;
				
		case TIMEOUT_CROSSMID:
			openmv.cross_flag = 0;
			timer.ms_count = 0;
			while(timer.ms_count  <=count &&
				  Gray_MidJudge() == 0)
					delay_ms(20);
	 		timer.ms_count = 0;
	 		openmv.cross_flag = 0;
			openmv.cross_num++;
				break;

		case PULSE:
//			openmv.cross_flag = 0;
//			timer.ms_count = 0;
//			encoder1.Pulse = 0;
//			encoder2.Pulse = 0;
//			while((encoder1.Pulse <=pulse) && (encoder2.Pulse <=pulse))
//					HAL_Delay(20);
//	 		encoder1.Pulse = 0;
//			encoder2.Pulse = 0;
//			timer.ms_count = 0;
//	 		openmv.cross_flag = 0;
//			openmv.cross_num++;
				break;

		case TIMEOUT_PULSE:
//			openmv.cross_flag = 0;
//			timer.ms_count = 0;
//			encoder1.Pulse = 0;
//			encoder2.Pulse = 0;
//			while(timer.count  <=ms_count &&
//				((encoder1.Pulse <=pulse) && (encoder2.Pulse <=pulse)))
//					delay_ms(20);
//	 		encoder1.Pulse = 0;
//			encoder2.Pulse = 0;
//			timer.ms_count = 0;
//	 		openmv.cross_flag = 0;
//			openmv.cross_num++;
				break;
						
		default:
			break;
	}
}



void APP_Straight(straight_mode mode, int Pulse, int timeout)
{
	switch (mode)
	{
		case ONE_FORWARD_GRAY:
			current_pid_mode = GRAYSCALE; 
			timer.state = STATE_PID_MODE;
			APP_stateJudge(1000,CROSS,0);
			MOTOR_Stop();
				break;
		
		case PULSE_:
			current_pid_mode = GRAYSCALE; 
			timer.state = STATE_PID_MODE;
			APP_stateJudge(0,PULSE,Pulse);
			MOTOR_Stop();
				break;
				
		case PULSE_GRAY_TURN:
			current_pid_mode = GRAYSCALE_TURN; 
			timer.state = STATE_PID_MODE;
			APP_stateJudge(0,PULSE,Pulse);
			MOTOR_Stop();
				break;
				
		case TIMEOUT_GRAY:
			current_pid_mode = GRAYSCALE; 
			timer.state = STATE_PID_MODE;
			APP_stateJudge(timeout,TIMEOUT,Pulse);
			MOTOR_Stop();
				break;

		case PULSE_GRAY:
				break;

		case SMALL_FORWARD:
				break;
	
		default:
				break;
	}
}

void APP_Rotate(rotate_mode mode, float target_angle)
{
	switch (mode)
	{
		case LEFT_90:
			timer.state = 0;
			current_pid_mode = OPEN_LOOP;
			timer.ms_count = 0;	
			// APP_stateJudge(4000,2,0);
			MOTOR_Stop();
				break;

		case RIGHT_90:
				break;

		case LEFT_180:
				break;

		case LEFT_PID:
			timer.mpu_flag = 1;		// 普通读取
			timer.state = STATE_PID_MODE;
			current_pid_mode = ANGLE_Left;
			target.yaw = target_angle;
			timer.ms_count = 0;	
			APP_stateJudge(2000,TIMEOUT,0);
			MOTOR_Stop();
				break;

		case RIGHT_PID:
				break;
				
		case LEFT_PID_FILTER:
			timer.mpu_flag = 2;		// 正滤波读取
			timer.state = STATE_PID_MODE;
			current_pid_mode = ANGLE_Left;
			target.yaw = target_angle;
			timer.ms_count = 0;	
			APP_stateJudge(2000,TIMEOUT,0);
			MOTOR_Stop();
				break;
				
		case RIGHT_PID_FILTER:
				break;
				
		default:
				break;
	}
}


void APP_Adjust(uint8_t mode, float target_angle, int time)
{
	switch (mode)
	{
		//角度调整
		case 1:
			// yaw_target = target_angle;
			// timer.mpu_flag = 1;
			// timer.flag = 2;
			// p_st_ang.En_All = 0;
			// PID_mode = 2;
			// HAL_Delay(time);
			
			// timer.flag = 0;
			// timer.mpu_flag = 1;
			// PID_mode = 1;
				break;
				
		//负滤波角度调整
		case 2:
			// yaw_target = target_angle;
			// timer.mpu_flag = 1;
			// timer.flag = 2;
			// p_st_ang.En_All = 0;
			// PID_mode = 3;
			// HAL_Delay(time);
			
			// timer.flag = 0;
			// timer.mpu_flag = 1;
			// PID_mode = 1;
				break;
				
		//正滤波角度调整
		case 3:
			// yaw_target = target_angle;
			// timer.mpu_flag = 1;
			// timer.flag = 2;
			// p_st_ang.En_All = 0;
			// PID_mode = 4;
			// HAL_Delay(time);
			
			// timer.flag = 0;
			// timer.mpu_flag = 1;
			// PID_mode = 1;
				break;
		
		default:
			break;
	}
}


void APP_Move(uint8_t object)
{
	switch (object)
	{

		case 1:
			OLED_ShowChar(2,15,'a');
			APP_Rotate(LEFT_PID, 90);
				break;

		case 2:
			OLED_ShowChar(2,15,'b');
			APP_Rotate(LEFT_PID_FILTER, 180);
				break;
				
		case 3:
			OLED_ShowChar(2,15,'c');
				break;
				
		case 4:
			OLED_ShowChar(2,15,'d');
				break;
		
		default:
				break;
	}
}


void APP_Task(void)
{
	if(KeyNum[0] >0)
	{
//		APP_Move(1);
//		memset(KeyNum, 0, 4);	// 数组清零
	}
	
	else if(KeyNum[1] >0)
	{
//		APP_Move(2);
//		memset(KeyNum, 0, 4);
	}
	
	else if(KeyNum[2] >0)
	{
//		APP_Move(3);
//		memset(KeyNum, 0, 4);
	}
	
	else if(KeyNum[3] >0)
	{
//		APP_Move(4);
//		memset(KeyNum, 0, 4);
	}
}



void APP_GPIO_Init(void)
{
//	PWR->CR |= 1<<8;//取消备份区写保护
//	RCC->BDCR &= 0xFFFFFFFE; //关闭外部低速振荡器，PC14，PC15成为普通IO  
////	BKP->CR &= 0xFFFFFFFE; //侵入检测TAMPER引脚(PC13)作为通用IO口使用
//	PWR->CR &= 0xFFFFFEFF; //备份区写保护
}